#include "cartographer/mapping/internal/global_trajectory_builder.h"
#include <memory>
#include "absl/memory/memory.h"
#include "absl/types/optional.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/internal/local_slam_result_data.h"
#include "cartographer/mapping/internal/motion_filter.h"
#include "cartographer/metrics/family_factory.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        namespace
        {
            static auto *kLocalSlamMatchingResults = metrics::Counter::Null();
            static auto *kLocalSlamInsertionResults = metrics::Counter::Null();
            /*
             * 该类可以认为是算法模块(局部建图和后端优化)与数据源之间的"桥梁"，其接收经过时间排序的传感器数据，其中只有IMU,SCAN,ODOM给到
             * 局部建图(进行初始位姿估计和地图匹配),所有数据都会给到后端优化模块。
             */
            template<typename LocalTrajectoryBuilder, typename PoseGraph>
            class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface
            {
            public:
                // Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no 'TimedPointCloudData' may be added in that case.
                GlobalTrajectoryBuilder(std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
                                        const int trajectory_id,
                                        PoseGraph *const pose_graph,
                                        const LocalSlamResultCallback &local_slam_result_callback,
                                        const absl::optional<MotionFilter> &pose_graph_odometry_motion_filter)
                    : trajectory_id_(trajectory_id),
                      pose_graph_(pose_graph),
                      local_trajectory_builder_(std::move(local_trajectory_builder)),
                      local_slam_result_callback_(local_slam_result_callback),
                      pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter)
                {
                }

                ~GlobalTrajectoryBuilder() override
                {
                }

                GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder &) = delete;

                GlobalTrajectoryBuilder &operator=(const GlobalTrajectoryBuilder &) = delete;

                // fundamental process
                void AddSensorData(const std::string &sensor_id, const sensor::TimedPointCloudData &timed_point_cloud_data) override
                {
                    CHECK(local_trajectory_builder_) << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
                    // std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
                    auto matching_result = local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);
                    // 调用函数AddRangeData完成Local SLAM的业务主线。如果一切正常， 就会返回扫描匹配的结果，
                    // 在该结果中同时记录了子图的更新信息。
                    if (matching_result == nullptr)
                    {
                        // The range data has not been fully accumulated yet.
                        return;
                    }
                    auto &insert_result = matching_result->insertion_result;
                    // 计数器自增，记录前端的输出次数。
                    kLocalSlamMatchingResults->Increment();
                    std::unique_ptr<InsertionResult> insertion_result;
                    if (insert_result != nullptr)
                    {
                        // 前端工作完成之后，将前端的输出结果喂给后端进行闭环检测和全局优化。
                        kLocalSlamInsertionResults->Increment();
                        auto node_id = pose_graph_->AddNode(
                            insert_result->constant_data,
                            trajectory_id_,
                            insert_result->insertion_submaps);
                        CHECK_EQ(node_id.trajectory_id, trajectory_id_);
                        insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
                            node_id,
                            insert_result->constant_data,
                            std::vector<std::shared_ptr<const Submap>>(
                                insert_result->insertion_submaps.begin(),
                                insert_result->insertion_submaps.end())});
                    }
                    if (local_slam_result_callback_)
                    {
                        // 将匹配结果传给外部(ROS node)进行可视化
                        local_slam_result_callback_(trajectory_id_, matching_result->time,
                                                    matching_result->local_pose,
                                                    std::move(matching_result->range_data_in_local),
                                                    std::move(insertion_result));
                    }
                }

                // 将数据喂给前端对象进行局部定位，然后通过后端的位姿图将传感器的信息添加到全局地图中。
                void AddSensorData(const std::string &sensor_id, const sensor::ImuData &imu_data) override
                {
                    if (local_trajectory_builder_)
                    {
                        local_trajectory_builder_->AddImuData(imu_data);
                    }
                    pose_graph_->AddImuData(trajectory_id_, imu_data);
                }

                void AddSensorData(const std::string &sensor_id, const sensor::OdometryData &odometry_data) override
                {
                    CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
                    if (local_trajectory_builder_)
                    {
                        local_trajectory_builder_->AddOdometryData(odometry_data);
                    }
                    // TODO(MichaelGrupp): Instead of having an optional filter on this level,
                    // odometry could be marginalized between nodes in the pose graph.
                    // Related issue: cartographer-project/cartographer/#1768
                    if (pose_graph_odometry_motion_filter_.has_value() && pose_graph_odometry_motion_filter_.value().IsSimilar(odometry_data.time, odometry_data.pose))
                    {
                        return;
                    }
                    pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
                }

                // 将类似GPS这种具有全局定位能力的传感器输出的位姿称为固定坐标系位姿(fixed frame pose)。
                // 由于它们的测量结果是全局的信息，所以没有传给前端用于局部定位。
                void AddSensorData(const std::string &sensor_id, const sensor::FixedFramePoseData &fixed_frame_pose) override
                {
                    if (fixed_frame_pose.pose.has_value())
                    {
                        CHECK(fixed_frame_pose.pose.value().IsValid())
                            << fixed_frame_pose.pose.value();
                    }
                    pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
                }

                void AddSensorData(const std::string &sensor_id, const sensor::LandmarkData &landmark_data) override
                {
                    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
                }

                void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) override
                {
                    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with local_trajectory_builder_ present.";
                    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
                }

            private:
                const int trajectory_id_;
                PoseGraph *const pose_graph_;
                std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
                LocalSlamResultCallback local_slam_result_callback_;
                absl::optional<MotionFilter> pose_graph_odometry_motion_filter_;
            };

        } // namespace

        std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
            std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
            const int trajectory_id, mapping::PoseGraph2D *const pose_graph,
            const TrajectoryBuilderInterface::LocalSlamResultCallback &
                local_slam_result_callback,
            const absl::optional<MotionFilter> &pose_graph_odometry_motion_filter)
        {
            return absl::make_unique<
                GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
                std::move(local_trajectory_builder), trajectory_id, pose_graph,
                local_slam_result_callback, pose_graph_odometry_motion_filter);
        }

        std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
            std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
            const int trajectory_id, mapping::PoseGraph3D *const pose_graph,
            const TrajectoryBuilderInterface::LocalSlamResultCallback &
                local_slam_result_callback,
            const absl::optional<MotionFilter> &pose_graph_odometry_motion_filter)
        {
            return absl::make_unique<
                GlobalTrajectoryBuilder<LocalTrajectoryBuilder3D, mapping::PoseGraph3D>>(
                std::move(local_trajectory_builder), trajectory_id, pose_graph,
                local_slam_result_callback, pose_graph_odometry_motion_filter);
        }

        void GlobalTrajectoryBuilderRegisterMetrics(metrics::FamilyFactory *factory)
        {
            auto *results = factory->NewCounterFamily(
                "mapping_global_trajectory_builder_local_slam_results",
                "Local SLAM results");
            kLocalSlamMatchingResults = results->Add({{"type", "MatchingResult"}});
            kLocalSlamInsertionResults = results->Add({{"type", "InsertionResult"}});
        }

    } // namespace mapping
} // namespace cartographer
